import pyrealsense2 as rs
import numpy as np
import cv2
import time
from collections import deque

def test_camera_fps(serial, width=640, height=480, fps=30, test_duration=10):
    """
    测试单个相机在指定参数下的实际帧率
    :param serial: 相机序列号
    :param width: 宽度
    :param height: 高度
    :param fps: 期望帧率
    :param test_duration: 测试持续时间(秒)
    :return: (是否成功, 平均帧率, 最小帧率, 最大帧率)
    """
    pipeline = rs.pipeline()
    config = rs.config()
    config.enable_device(serial)
    
    try:
        # 尝试配置流
        config.enable_stream(rs.stream.color, width, height, rs.format.bgr8, fps)
        config.enable_stream(rs.stream.depth, width, height, rs.format.z16, fps)
        
        # 启动管道
        pipeline.start(config)
        print(f"相机 {serial} 启动成功，开始{test_duration}秒帧率测试...")
        
        # 用于存储帧时间戳
        frame_timestamps = deque()
        start_time = time.time()
        
        while time.time() - start_time < test_duration:
            # 等待帧
            frames = pipeline.wait_for_frames()
            color_frame = frames.get_color_frame()
            depth_frame = frames.get_depth_frame()
            
            if not color_frame or not depth_frame:
                continue
                
            # 记录当前时间戳
            frame_timestamps.append(time.time())
            
            # 显示图像（可选）
            color_image = np.asanyarray(color_frame.get_data())
            cv2.putText(color_image, f"FPS: Calculating...", (10, 30), 
                        cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
            cv2.imshow(f"Camera {serial} (640x480)", color_image)
            
            # 按ESC退出测试
            if cv2.waitKey(1) == 27:
                break
        
        # 停止管道
        pipeline.stop()
        cv2.destroyWindow(f"Camera {serial} (640x480)")
        
        # 计算帧率
        if len(frame_timestamps) < 2:
            return (False, 0, 0, 0)
            
        # 计算每两帧之间的时间间隔
        intervals = []
        for i in range(1, len(frame_timestamps)):
            intervals.append(frame_timestamps[i] - frame_timestamps[i-1])
            
        # 计算帧率统计
        avg_fps = len(intervals) / (frame_timestamps[-1] - frame_timestamps[0])
        min_fps = 1.0 / max(intervals) if intervals else 0
        max_fps = 1.0 / min(intervals) if intervals else 0
        
        return (True, avg_fps, min_fps, max_fps)
        
    except Exception as e:
        print(f"相机 {serial} 配置失败: {str(e)}")
        return (False, 0, 0, 0)

def main():
    # 获取所有连接的RealSense设备
    ctx = rs.context()
    devices = ctx.query_devices()
    
    if len(devices) == 0:
        print("未检测到任何RealSense相机")
        return
        
    print(f"检测到 {len(devices)} 个RealSense相机，开始640x480@30Hz测试...")
    print("测试将持续10秒，可按ESC提前结束单个相机测试")
    
    # 存储测试结果
    results = []
    
    # 逐个测试相机
    for device in devices:
        serial = device.get_info(rs.camera_info.serial_number)
        model = device.get_info(rs.camera_info.name)
        print(f"\n测试相机: {model} (序列号: {serial})")
        
        success, avg_fps, min_fps, max_fps = test_camera_fps(
            serial, 
            width=640, 
            height=480, 
            fps=30,
            test_duration=10
        )
        
        results.append({
            "serial": serial,
            "model": model,
            "success": success,
            "avg_fps": avg_fps,
            "min_fps": min_fps,
            "max_fps": max_fps
        })
        
        # 显示测试结果
        if success:
            print(f"测试成功 - 平均帧率: {avg_fps:.2f} FPS, "
                  f"最小帧率: {min_fps:.2f} FPS, "
                  f"最大帧率: {max_fps:.2f} FPS")
            if avg_fps >= 28:  # 允许一定误差
                print("符合640x480@30Hz要求")
            else:
                print("帧率不足，不符合640x480@30Hz要求")
        else:
            print("测试失败，不支持640x480@30Hz配置")
    
    # 汇总结果
    print("\n===== 测试汇总 =====")
    for res in results:
        status = "通过" if (res["success"] and res["avg_fps"] >= 28) else "未通过"
        print(f"{res['model']} ({res['serial']}): {status} "
              f"(平均帧率: {res['avg_fps']:.2f} FPS)")

if __name__ == "__main__":
    main()

